import os
import subprocess
import multiprocessing
from my_common_lib import find_file_in_folder, find_files_in_folder
# 定义参数
# anchors_config_paths = [
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\39\mixed6_optimal_with_4_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\38\mixed5_optimal_with_4_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\37\mixed4_optimal_with_4_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\36\mixed3_optimal_with_4_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\35\mixed2_optimal_with_4_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\34\mixed_optimal_with_4_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27\position_optimal_with_4_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison\pose_optimal_with_41_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison\pose_optimal_with_42_anchors.txt",
#     r"C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison\pose_optimal_with_43_anchors.txt",
# ]

# anchors_config_folders = [
#         r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27',
#         # r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\56',
#         # r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
#         # r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
#         # r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
# ]
# anchors_config_paths = find_files_in_folder(anchors_config_folders, "*_optimal_with_*_anchors.txt")

# dynamic test
anchors_config_paths = []
# dynamic_obj_folder_id  = [63, 64, 65, 66, 67, 68, 69, 70, 71, 72]
# dynamic_obj_folder_id  = [73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, '41_comparison_fullpath', '42_comparison_fullpath', '43_comparison_fullpath']
# dynamic_obj_folder_id  = [95, 96, 9+7, 98]
# dynamic_obj_folder_id  = [64, 95, 96, 97, 98,  '41_comparison', '42_comparison', '43_comparison'] # 
dynamic_obj_folder_id  = ['6_2_comparison', '6_3_comparison', '6_4_comparison', '6_5_comparison'] # for different size comparison
for i in range(len(dynamic_obj_folder_id)):
    anchors_config_folder = os.path.join(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma', str(dynamic_obj_folder_id[i]))
    anchors_config_path = find_file_in_folder(anchors_config_folder, "*_optimal_with_*_anchors.txt")
    anchors_config_paths.append(anchors_config_path)


script_path = r".\ekf_test.py"  # 脚本路径
num_iterations = 100  # 每个配置运行的次数

# 循环运行脚本
def run_script(config_path):
    for i in range(num_iterations):
        print(f"Running iteration {i + 1}/{num_iterations} for config: {config_path}")
        # 构造命令
        command = [
            "python", 
            script_path, 
            "--anchors_config_path", 
            config_path
        ]
        # 执行命令
        subprocess.run(command)
        print(f"Completed iteration {i + 1}/{num_iterations} for config: {config_path}")

if __name__ == '__main__':
    # 使用进程池，运行每两个进程并行
    with multiprocessing.Pool(processes=len(dynamic_obj_folder_id)) as pool:
        pool.map(run_script, anchors_config_paths)  # 使用进程池同时运行多个脚本